package com.mydomain;
import java.util.ArrayList;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.EOPD;


public class Labyrinth {
        private static int rotationItCnt = 0;
        
        static NXTRegulatedMotor leftMotor = Motor.A;
        static NXTRegulatedMotor rightMotor = Motor.B;
        static NXTRegulatedMotor frontMotor = Motor.C;
        
        static volatile float leftDistance, frontDistance, rightDistance;
        static ArrayList<Thread> threads = new ArrayList<Thread>();
        
        
        public static void main(String[] args) throws InterruptedException {
                startThreads();
        }

        private static void startThreads() {
                Thread rotMotorThread = new Thread() {
                        @Override
                        public void run() {
                                try { 
                                        rotateUltraSensor();
                                } catch (Exception e) {}
                        };
                };
                rotMotorThread.start();
                threads.add(rotMotorThread);
                
                Thread displayOutputThread = new Thread() {
                        @Override
                        public void run() {
                                try {
                                        displayOutput();
                                } catch (Exception e) {}
                        }
                };
                displayOutputThread.start();
                threads.add(displayOutputThread);
                
                Thread driveThread = new Thread() {
                        @Override
                        public void run() {
                                try {
                                        drive();
                                } catch (Exception e) {}
                        }
                };
                driveThread.start();
                threads.add(driveThread);
        }
        
        private static void exitAll() {
                leftMotor.stop();
                rightMotor.stop();
                frontMotor.stop();
                
                for (Thread t : threads) {
                        t.interrupt();
                }
        }

        private static void displayOutput() throws InterruptedException {
                while (true) {
                        LCD.clear();
                        
                        LCD.drawString("US links: " + leftDistance, 0, 1);
                        LCD.drawString("US vorne: " + frontDistance, 0, 2);
                        LCD.drawString("US rechts: " + rightDistance, 0, 3);
                        LCD.drawString("LEFT: "+leftMotor.getRotationSpeed(), 0, 4);
                        LCD.drawString("RIGHT: "+rightMotor.getRotationSpeed(), 0, 5);
                        
                        Thread.sleep(250);
                }
        }
        
        private static void rotateUltraSensor() throws InterruptedException {
        		NXTRegulatedMotor rotMotor  = frontMotor;
                UltrasonicSensor usSensor = new UltrasonicSensor(SensorPort.S1);
                
                rotMotor.setSpeed(250);
                int sleep = 125;
                
                while (true) {
                        if (rotationItCnt == 5) {
                                rotMotor.rotate(-5);
                        }

                        rotMotor.rotate(91);
                        Labyrinth.rightDistance = usSensor.getRange()-4;
                        Thread.sleep(sleep);
                        
                        rotMotor.rotate(-182);
                        Labyrinth.leftDistance = usSensor.getRange()+4;
                        Thread.sleep(sleep);
                        
                        rotMotor.rotate(91);
                        Labyrinth.frontDistance = usSensor.getRange();
                        Thread.sleep(sleep);
                }
        }
        
        private static void drive() throws InterruptedException {
                final int defaultMotorSpeed = 85;
                Thread.sleep(4000);
                
                int leftMotorSpeed = defaultMotorSpeed;
                int rightMotorSpeed = defaultMotorSpeed;
                
                NXTRegulatedMotor motorLinks = Motor.B;
                NXTRegulatedMotor motorRechts = Motor.A;
                
                motorLinks.setSpeed(leftMotorSpeed);
                motorRechts.setSpeed(rightMotorSpeed);
                
                motorLinks.forward();
                motorRechts.forward();
                
                do {
                        float distanceDiff = Labyrinth.leftDistance - Labyrinth.rightDistance;
                        if ( distanceDiff > 1) {
                                rightMotorSpeed = Math.min((int) (defaultMotorSpeed + distanceDiff*2), 125);
                                leftMotorSpeed = Math.min((int) (defaultMotorSpeed - distanceDiff*2), 125);
                                
                        } else if (distanceDiff < -1) {
                                leftMotorSpeed = Math.min((int) (defaultMotorSpeed - distanceDiff*2), 125);
                                rightMotorSpeed = Math.min((int) (defaultMotorSpeed + distanceDiff*2), 125);
                        } else {
                                leftMotorSpeed = defaultMotorSpeed;
                                rightMotorSpeed = defaultMotorSpeed;
                        }
                        
                        motorLinks.setSpeed(leftMotorSpeed);
                        motorRechts.setSpeed(rightMotorSpeed);
                        Thread.sleep(250);
                } while (frontDistance >= 18.0f);
                
                exitAll();
        }
        
 }
